#include <CyberLib.h>
//#include <SoftwareSerial.h>
int LED = 13;      // LED подключен к 12 пину
int leftDirPin = 4;
int leftSpeedPin = 5;
int rightDirPin = 7;
int rightSpeedPin = 6;

int din=11;  
int sonarTrig=14;  
int sonarEhcho=15;  

int runSpeedHIGH = 225;
int runSpeedLOW = 50;


#define size_buff 5 //размер массива sensor
uint16_t sensor[size_buff]; //массив для хранения замеров дальномера
uint8_t stat=0; //направление разворота
//uint16_d stat=0; //дистанция
//SoftwareSerial btSerial(9,10);

void setup()  
{
  pinMode(leftDirPin, OUTPUT);//инициализация выходов моторов
  pinMode(leftSpeedPin, OUTPUT);
  pinMode(rightDirPin, OUTPUT);
  pinMode(rightSpeedPin, OUTPUT);
  pinMode(din, OUTPUT); //динамик
  digitalWrite(din, LOW);
  pinMode(sonarTrig, OUTPUT); //пин trig ультразвукового сонара
  digitalWrite(sonarTrig, LOW);
  pinMode(sonarEhcho, INPUT); //пин echo  ультразвукового сонара
  //randomSeed(A6_Read); //Получить случайное значение
  //for(uint8_t i=0; i<12; i++) beep(70, random(100, 2000)); //звуковое оповещение готовности робота
  //wdt_enable(WDTO_500MS);    //Сторожевая собака 0,5сек.
  //btSerial.begin(9600);
  pinMode(LED, OUTPUT); //светодиод
  Serial.begin(9600);   // инициализация порта для тестов
}

void loop()
{  
  uint16_t dist=GetDistance(); //производим замер дистанции
    if( dist < 10) {
     //rotation(stat, 255);
       digitalWrite(LED, LOW);
  } else   //если 10см максимальный угол разворота
    if( dist < 20) {
      //rotation(stat, 200);
       digitalWrite(LED, LOW);
} else   //если 20см  средний угол разворота
    if( dist < 40) {
      //rotation(stat, 130);
       digitalWrite(LED, LOW);
  } else   //если 40см  минимальный угол разворота
        //digitalWrite(leftDirPin, HIGH);
        //digitalWrite(rightDirPin, HIGH);
        //analogWrite(leftSpeedPin, runSpeedLOW);
        //analogWrite(rightSpeedPin, runSpeedLOW);
       stat=~stat;
       //digitalWrite(LED, HIGH);
     Serial.println(dist);
     //wdt_reset(); //покормить собаку
}
//***************************************************
void rotation(uint8_t arr, uint8_t dur) 
{
    switch (arr) //смотрим в каком направление разворачиваться
    {
    case 0:
      digitalWrite(leftDirPin, LOW);
      digitalWrite(rightDirPin, HIGH);
      analogWrite(leftSpeedPin, runSpeedHIGH);
      analogWrite(rightSpeedPin, runSpeedLOW);
      break;
    case 255:
        digitalWrite(leftDirPin, HIGH);
        digitalWrite(rightDirPin, LOW);
        analogWrite(leftSpeedPin, runSpeedLOW);
        analogWrite(rightSpeedPin, runSpeedHIGH);
      break;    
    } 
    delay(dur);    //угол разворота
      digitalWrite(leftDirPin, LOW);
      digitalWrite(rightDirPin, LOW);
      analogWrite(leftSpeedPin, runSpeedLOW);
      analogWrite(rightSpeedPin, runSpeedLOW);//стоп мотор!
}
//***************************************************
uint16_t GetDistance() 
{
 uint16_t dist;
 uint8_t maxcomp=0; //счётчик максимального колличества совпадений
 uint16_t mcn;	//максимально часто встречающийся элемент массива
 uint16_t comp;	//временная переменная
 for (uint8_t i = 0; i < size_buff; ++i) //производим несколько замеров
  { 
   digitalWrite(sonarTrig, HIGH); //запустить измерение
   delayMicroseconds(10);
   digitalWrite(sonarTrig, LOW);  
   dist = pulseIn(sonarEhcho, HIGH, 2400); //считываем длительность времени прохождения эха, ограничить время ожидания
   if(dist==0) dist=2400;  
   sensor[i]=dist;  //сохранить в массиве
   delay(40); //задержка между посылками
   //wdt_reset(); //покормить собаку, что бы она не сбежала 
  }
  //dist=(find_similar(sensor, size_buff, 58))/58; // //фильтруем показания датчика и переводим в см
 	for (uint8_t i=0; i<size_buff; i++) 
	{
		comp=sensor[i];	//кладем элемент массива в comp
		uint8_t n=0;	//счётчик совпадении
		for (uint8_t j=0; j<size_buff; j++)	{ if (sensor[j]>comp-58 && sensor[j]<comp+58) n++;} // ищем повторения элемента comp в массиве buf	
		if (n > maxcomp) //если число повторов больше чем было найдено ранее
		{
			maxcomp=n; //сохраняем счетяик повторов
			mcn=comp; //сохраняем повторяемый элемент
		}		
	}
 return mcn/58;
}
//***************************************************
